Joint on-manifold self-calibration of odometry model and sensor extrinsics using pre-integration

© 2019 IEEE. Personal use of this material is permitted. Permission from IEEE must be obtained for all other uses, in any current or future media, including reprinting/republishing this material for advertising or promotional purposes, creating new collective works, for resale or redistribution to servers or lists, or reuse of any copyrighted component of this work in other works.This paper describes a self-calibration procedure that jointly estimates the extrinsic parameters of an exteroceptive sensor able to observe ego-motion, and the intrinsic parameters of an odometry motion model, consisting of wheel radii and wheel separation. We use iterative nonlinear onmanifold optimization with a graphical representation of the state, and resort to an adaptation of the pre-integration theory, initially developed for the IMU motion sensor, to be applied to the differential drive motion model. For this, we describe the construction of a pre-integrated factor for the differential drive motion model, which includes the motion increment, its covariance, and a first-order approximation of its dependence with the calibration parameters. As the calibration parameters change at each solver iteration, this allows a posteriori factor correction without the need of re-integrating the motion data. We validate our proposal in simulations and on a real robot and show the convergence of the calibration towards the true values of the parameters. It is then tested online in simulation and is shown to accommodate to variations in the calibration parameters when the vehicle is subject to physical changes such as loading and unloading a freight.Peer ReviewedPostprint (author's final draft

Word ordering and document adjacency for large loop closure detection in 2D laser maps

© 20xx IEEE. Personal use of this material is permitted. Permission from IEEE must be obtained for all other uses, in any current or future media, including reprinting/republishing this material for advertising or promotional purposes, creating new collective works, for resale or redistribution to servers or lists, or reuse of any copyrighted component of this work in other worksWe address in this paper the problem of loop closure detection for laser-based simultaneous localization and mapping (SLAM) of very large areas. Consistent with the state of the art, the map is encoded as a graph of poses, and to cope with very large mapping capabilities, loop closures are asserted by comparing the features extracted from a query laser scan against a previously acquired corpus of scan features using a bag-ofwords (BoW) scheme. Two contributions are here presented. First, to benefit from the graph topology, feature frequency scores in the BoW are computed not only for each individual scan but also from neighboring scans in the SLAM graph. This has the effect of enforcing neighbor relational information during document matching. Secondly, a weak geometric check that takes into account feature ordering and occlusions is introduced that substantially improves loop closure detection performance. The two contributions are evaluated both separately and jointly on four common SLAM datasets, and are shown to improve the state-of-the-art performance both in terms of precision and recall in most of the cases. Moreover, our current implementation is designed to work at nearly frame rate, allowing loop closure query resolution at nearly 22 Hz for the best case scenario and 2 Hz for the worst case scenario.Peer ReviewedPostprint (author's final draft

WOLF: A modular estimation framework for robotics based on factor graphs

© 2022 IEEE. Personal use of this material is permitted. Permission from IEEE must be obtained for all other uses, in any current or future media, including reprinting/republishing this material for advertising or promotional purposes,creating new collective works, for resale or redistribution to servers or lists, or reuse of any copyrighted component of this work in other works.This paper introduces WOLF, a C++ estimation framework based on factor graphs and targeted at mobile robotics. WOLF can be used beyond SLAM to handle self-calibration, model identification, or the observation of dynamic quantities other than localization. The architecture of WOLF allows for a modular yet tightly-coupled estimator. Modularity is enhanced via reusable plugins that are loaded at runtime depending on application setup. This setup is achieved conveniently through YAML files, allowing users to configure a wide range of applications without the need of writing or compiling code. Most procedures are coded as abstract algorithms in base classes with varying levels of specialization. Overall, all these assets allow for coherent processing and favor code re-usability and scalability. WOLF can be used with ROS, and is made publicly available and open to collaboration.Peer ReviewedPostprint (author's final draft

Robust Navigation for Industrial Service Robots

Tesis llevada a cabo para conseguir el grado de Doctor por la Universidad Politécnica de Cataluña.--2020-09-29As one of the fundamental problems of robotics, the different challenges that constitute navigation have been studied for decades. Robust, reliable and safe navigation is a key factor for the enablement of higher level functionalities for robots that are going to evolve around humans on a daily basis. Throughout the present thesis, we tackle the problem of navigation for robotic industrial mobile-bases. We identify its components and analyze their respective challenges in order to address them. The research work presented here ultimately aims at improving the overall quality of the navigation stack of a commercially available industrial mobile-base. To introduce and survey the overall problem we first break down the navigation framework into clearly identified smaller problems. We examine the problem of simultaneously mapping the environment and localizing the robot in it by exploring the state of the art. Doing so we recall and detail the mathematical grounding of the Simultaneous Localization and Mapping (SLAM) problem. We then review the problem of planning the trajectory of a mobile-base toward a desired goal in the generated environment representation. Finally we investigate and clarify the concepts and mathematical tools of the Lie theory, which we use extensively to provide rigorous mathematical foundation to our developments, focusing on the subset of the theory that is useful to state estimate in robotics. As the first identified space for improvements, the problem of place recognition for closing loops in SLAM is addressed. Loop closure concerns the ability of a robot to recognize a previously visited location and infer geometrical information between its current and past locations. Using only a 2D laser range finder sensor, the task is challenging as the perception of the environment provided by the sensor is sparse and limited. We tackle the problem using a technique borrowed from the field of Natural Language Processing (NLP) which has been successfully applied to image-based place recognition, namely the Bag-of-Words. We further improve the method with two proposals inspired from NLP. Firstly the comparison of places is strengthen by taking into account the natural relative order of features in each individual sensor readings. Secondly, topological correspondences between places in a corpus of visited places are established in order to promote together instances that are ‘close’ to one another. We evaluate both our proposals separately and jointly on several data sets, with and without noise, and show an improvement over the state of the art

Manif: A micro Lie theory library for state estimation in robotics applications

There has been a remarkable effort in the last years in the robotics community to formulate estimation problems properly (Eade, 2013)(Barfoot, 2017). This is motivated by an increasing demand for precision, consistency, and stability of the solutions. Indeed, proper modeling of the states and measurements, the functions relating them, and their uncertainties, is crucial to achieve these goals. This has led to problem formulations involving what has been known as ‘manifolds’, which in this context are no less than the smooth topologic surfaces of the Lie groups where the state representations evolve (Chirikjian, 2011). manif (Deray & Solà, 2019) is a micro Lie theory library targeted at state estimation in robotics applications. With a single dependency on Eigen (Guennebaud, Jacob, & others, 2010) and a requirement on C++11 only, it is developed as a header-only library, making it easy to integrate to existing projects. The manif library provides simple interfaces to the most common operations on Lie groups in state estimation. Its design is similar to Eigen, in that all Lie group classes inherit from a templated base class using static polymorphism. This allows for writing generic code without paying the price of pointer arithmetic. Thanks to this polymorphism, the library is open to extensions to Lie groups beyond the currently implemented: the Special Orthogonal groups SO(2) and SO(3) and the Special Euclidean groups SE(2) and SE(3). The mathematical foundations of the library are given in (Solà, Deray, & Atchuthan, 2018), which is often referred to in the documentation, especially for providing references for the mathematical formulae

Joint on-manifold self-calibration of odometry model and sensor extrinsics using pre-integration

Trabajo presentado en la European Conference on Mobile Robots (ECMR), celebrada en Praga (República Checa), del 4 al 6 de septiembre de 2019This paper describes a self-calibration procedure that jointly estimates the extrinsic parameters of an exteroceptive sensor able to observe ego-motion, and the intrinsic parameters of an odometry motion model, consisting of wheel radii and wheel separation. We use iterative nonlinear onmanifold optimization with a graphical representation of the state, and resort to an adaptation of the pre-integration theory, initially developed for the IMU motion sensor, to be applied to the differential drive motion model. For this, we describe the construction of a pre-integrated factor for the differential drive motion model, which includes the motion increment, its covariance, and a first-order approximation of its dependence with the calibration parameters. As the calibration parameters change at each solver iteration, this allows a posteriori factor correction without the need of re-integrating the motion data. We validate our proposal in simulations and on a real robot and show the convergence of the calibration towards the true values of the parameters. It is then tested online in simulation and is shown to accommodate to variations in the calibration parameters when the vehicle is subject to physical changes such as loading and unloading a freight.This work has been supported by the Spanish Ministry of Science, Innovation, and Universities project EB-SLAM (DPI2017-89564-P), by the EU H2020 project LOGIMATIC (H2020-Galileo-2015-1-687534) and by the Spanish State Research Agency through the Maria de Maeztu Seal of Excellence to IRI MDM-2016-0656. J. Deray acknowledges support from the Industrial Doctorate Program of the Catalan Agency for Management of University and Research Grants

A micro Lie theory for state estimation in robotics

A Lie group is an old mathematical abstract object dating back to the XIX century, when mathematician Sophus Lie laid the foundations of the theory of continuous transformation groups. As it often happens, its usage has spread over diverse areas of science and technology many years later. In robotics, we are recently experiencing an important trend in its usage, at least in the fields of estimation, and particularly in motion estimation for navigation. Yet for a vast majority of roboticians, Lie groups are highly abstract constructions and therefore difficult to understand and to use. This may be due to the fact that most of the literature on Lie theory is written by and for mathematicians and physicists, who might be more used than us, perhaps by their academic formation, to the deep abstractions this theory deals with. In estimation for robotics, it is often not necessary to exploit the full capacity of the theory, and therefore an effort of selection of materials is required. In this paper, we will walk through the most basic principles of the Lie theory, with the aim of conveying clear and useful ideas, and leave a significant corpus of the Lie theory behind. Even with this mutilation, the material included here has proven to be extremely useful in modern estimation algorithms for robotics, especially in the fields of SLAM, visual odometry, and the like. We provide also a vast reference of formulas for the major groups used in robotics, including most jacobian matrices and the way to easily manipulate them

Word ordering and document adjacency for large loop closure detection in 2D laser maps

We address in this paper the problem of loop closure detection for laser-based simultaneous localization and mapping (SLAM) of very large areas. Consistent with the state of the art, the map is encoded as a graph of poses, and to cope with very large mapping capabilities, loop closures are asserted by comparing the features extracted from a query laser scan against a previously acquired corpus of scan features using a bag-ofwords (BoW) scheme. Two contributions are here presented. First, to benefit from the graph topology, feature frequency scores in the BoW are computed not only for each individual scan but also from neighboring scans in the SLAM graph. This has the effect of enforcing neighbor relational information during document matching. Secondly, a weak geometric check that takes into account feature ordering and occlusions is introduced that substantially improves loop closure detection performance. The two contributions are evaluated both separately and jointly on four common SLAM datasets, and are shown to improve the state-of-the-art performance both in terms of precision and recall in most of the cases. Moreover, our current implementation is designed to work at nearly frame rate, allowing loop closure query resolution at nearly 22 Hz for the best case scenario and 2 Hz for the worst case scenario.This work was supported by the Spanish Ministry of Economy and Competitiveness project ROBINSTRUCT TIN2014-58178-R, the EU H2020 LOGMATIC project 687534, and the Industrial Doctoral program of the Catalan Agency for Management of University and Research Grants.Peer Reviewe

Hand Gestures Recognition and Tracking

In this project we develop a system that uses low cost web cameras to recognise gestures and track 2D orientations of the hand. This report is organized as such. First in section 2 we introduce various methods we undertook for hand detection. This is the most important step in hand gesture recognition. Results of various skin detection algorithms are discussed in length. This is followed by region extraction step (section 3). In this section approaches like contours and convex hull to extract region of interest which is hand are discussed. In section 4 a method is describe to recognize the open hand gesture. Two additional gestures of palm and fist are implemented using Haar-like features. These are discussed in section 5. In section 6 Kalman filter is introduced which tracks the centroid of hand region. The report is concluded by discussing about various issues related with the embraced approach (section 9) and future recommendations to improve the system is pointed out (section 10)